#include "sc.h"
 
NeighbourhoodToPixelOp()
shift(in stream unsigned char i[0][0..16],
      in int *val,
      out stream unsigned char *o)
{
  *o = i[0][*val];
}

NeighbourhoodToPixelOp()
sad(in stream unsigned char i1[-2..2][-2..2],
    in stream unsigned char i2[-2..2][-2..2],
    out stream unsigned char *o)
{
  int sad=0, y, x;

  for (y=-2; y < 3; y++)
    for (x=-2; x < 3; x++)
      sad = sad + abs(i1[y][x] - i2[y][x]);

  *o = sad/25;
}

PixelToPixelOp()
dispinit(in stream unsigned char *diff1,
         in stream unsigned char *diff2,
         in int *shift1,
         in int *shift2,
         out stream unsigned char *err,
         out stream unsigned char *disp)
{
  if (*diff2 < *diff1)
  {
    *disp = *shift2;
    *err = *diff2;
  }
  else
  {
    *disp = *shift1;
    *err = *diff1;
  }
}

PixelToPixelOp()
dispupdate(in stream unsigned char *diff1,
           in stream unsigned char *diff2,
           in stream unsigned char *shift1,
           in stream unsigned char *shift2,
           out stream unsigned char *err,
           out stream unsigned char *disp)
{
  if (*diff2 < *diff1)
  {
    *disp = *shift2;
    *err = *diff2;
  }
  else
  {
    *disp = *shift1;
    *err = *diff1;
  }
}

PixelToPixelOp()
mul(in stream unsigned char *i,
    in int *val,
    out stream unsigned char *o)
{
  *o = *i * *val;
}

#define SHIFTS 16

MAINTYPE MAIN(int argc, char **argv)
{
  int mean, i;
  STREAM left, right, shifted, difference[16], disparity;
  char text[64];

  scInit(argc, argv);

  generate(scint(1023), &left);  _multi(&left);
  generate(scint(1024), &right); _multi(&right);
  
  for (i=0; i < SHIFTS; i++)
  {
    shift(&left, &i, &shifted);
    sad(&shifted, &right, &difference[i]);
  }

  if (SHIFTS > 1)
    for (i=0; i < SHIFTS; i+=2)
      dispinit(&difference[i], &difference[i+1], scint(i), scint(i+1), &difference[i], &difference[i+1]);
  if (SHIFTS > 2)
    for (i=0; i < SHIFTS; i+=4)
      dispupdate(&difference[i], &difference[i+2], &difference[i+1], &difference[i+3], &difference[i], &difference[i+1]);
  if (SHIFTS > 4)
    for (i=0; i < SHIFTS; i+=8)
      dispupdate(&difference[i], &difference[i+4], &difference[i+1], &difference[i+5], &difference[i], &difference[i+1]);
  if (SHIFTS > 8)
    for (i=0; i < SHIFTS; i+=16)
      dispupdate(&difference[i], &difference[i+8], &difference[i+1], &difference[i+9], &difference[i], &difference[i+1]);
    
  discard(&difference[0]);
  mul(&difference[1], scint(16), &disparity);
  discard(&disparity);

  return scExit();
}
